#include <motor/servo_newstart.h>

namespace ros_canopen_driver {

ServoNewstart::ServoNewstart(std::shared_ptr<rclcpp::Node> node, const std::string name)
{
    node_ = node;
    joint_name = name;
    servo_manufacturer = NEWSTART;
    memset(&m_servo_motor, 0, sizeof(SERVOMOTOR_PORT));
    // pCanopen = Canopen::getInstance();
    preTargetEnc = -1;
    pos_send_scale = 0;
    motorState = Off;
    pc_.timestamp = node_->now().seconds();
}

ServoNewstart::~ServoNewstart()
{
    // delete pCanopen;
    // pCanopen = nullptr;
}

/*Description: load motor configuration parameters from yaml file*/
bool ServoNewstart::Motor_InitParam()
{
    if (!node_->has_parameter(joint_name + "/nodeId")) {
        RCLCPP_ERROR_STREAM(node_->get_logger(), joint_name << " nodeId is invalid");
        return false;
    }
    int nodeId;
    node_->get_parameter(joint_name + "/nodeId", nodeId);
    m_servo_motor.cfg.nodeId = nodeId;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " nodeId is " << nodeId);
    
    node_->get_parameter_or(joint_name + "/busname", busname, std::string("can0"));
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " busname: " << busname);
    
    node_->get_parameter_or(joint_name + "/type", servo_type, 0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " type: " << servo_type);
    
    node_->get_parameter_or(joint_name + "/ratio", ratio, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " ratio: " << ratio);
    
    node_->get_parameter_or(joint_name + "/pos_scale", pos_scale, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " pos_scale: " << pos_scale);
    
    node_->get_parameter_or(joint_name + "/pos_offset", pos_offset, 0.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " pos_offset: " << pos_offset);
    
    node_->get_parameter_or(joint_name + "/Ipeak", ipeak, 20.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " Ipeak: " << ipeak);
    
    node_->get_parameter_or(joint_name + "/use_extra_encoder", use_extra_encoder, false);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " use external encoder: " << use_extra_encoder);
    
    node_->get_parameter_or(joint_name + "/rad_to_meter", rad_to_meter, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " rad_to_meter: " << rad_to_meter);
    int ctl_mode;
    node_->get_parameter_or(joint_name + "/ctl_mode", ctl_mode, 0);
    m_servo_motor.cfg.ctl_mode = ctl_mode;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " ctl_mode is " << ctl_mode);
    
    node_->get_parameter_or(joint_name + "/rotateReverse", m_servo_motor.cfg.rotateReverse, false);
    node_->get_parameter_or(joint_name + "/reduction_ratio", m_servo_motor.cfg.reduction_ratio, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " reduction_ratio is " << m_servo_motor.cfg.reduction_ratio);
    
    node_->get_parameter_or(joint_name + "/encoder_resolution", m_servo_motor.cfg.encoder_resolution, 10000.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " encoder_resolution is " << m_servo_motor.cfg.encoder_resolution);
    // 速度控制模式配置
    int velocity_mode_acc;
    node_->get_parameter_or(joint_name + "/vModeCfg/acc", velocity_mode_acc, 0);
    m_servo_motor.cfg.vModeCfg.acc = velocity_mode_acc;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " velocity_mode_acc is " << m_servo_motor.cfg.vModeCfg.acc);
    
    int velocity_mode_dec;
    node_->get_parameter_or(joint_name + "/vModeCfg/dec", velocity_mode_dec, 0);
    m_servo_motor.cfg.vModeCfg.dec = velocity_mode_dec;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " velocity_mode_dec is " << m_servo_motor.cfg.vModeCfg.dec);
    
    // 位置控制模式配置
    int position_mode_vel;
    node_->get_parameter_or(joint_name + "/pModeCfg/vel", position_mode_vel, 0);
    m_servo_motor.cfg.pModeCfg.vel = position_mode_vel;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " position_mode_vel is " << m_servo_motor.cfg.pModeCfg.vel);
    
    int position_mode_acc;
    node_->get_parameter_or(joint_name + "/pModeCfg/acc", position_mode_acc, 0);
    m_servo_motor.cfg.pModeCfg.acc = position_mode_acc;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " position_mode_acc is " << m_servo_motor.cfg.pModeCfg.acc);
    
    int position_mode_dec;
    node_->get_parameter_or(joint_name + "/pModeCfg/dec", position_mode_dec, 0);
    m_servo_motor.cfg.pModeCfg.dec = position_mode_dec;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " position_mode_dec is " << m_servo_motor.cfg.pModeCfg.dec);
    
    // 原点配置
    node_->get_parameter_or(joint_name + "/homeNeed", m_servo_motor.cfg.homeNeed, false);
    if (m_servo_motor.cfg.homeNeed) {
        int home_mode;
        node_->get_parameter_or(joint_name + "/hModeCfg/home_mode", home_mode, 0);
        m_servo_motor.cfg.hModeCfg.home_mode = home_mode;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " home_mode is " << home_mode);
        
        int home_mode_velH;
        node_->get_parameter_or(joint_name + "/hModeCfg/velH", home_mode_velH, 0);
        m_servo_motor.cfg.hModeCfg.velH = home_mode_velH;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " home_mode_velH is " << m_servo_motor.cfg.hModeCfg.velH);
        
        int home_mode_velL;
        node_->get_parameter_or(joint_name + "/hModeCfg/velL", home_mode_velL, 0);
        m_servo_motor.cfg.hModeCfg.velL = home_mode_velL;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " home_mode_velL is " << m_servo_motor.cfg.hModeCfg.velL);
        
        int home_mode_acc;
        node_->get_parameter_or(joint_name + "/hModeCfg/acc", home_mode_acc, 0);
        m_servo_motor.cfg.hModeCfg.acc = home_mode_acc;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " home_mode_acc is " << m_servo_motor.cfg.hModeCfg.acc);
        
        node_->get_parameter_or(joint_name + "/hModeCfg/homeOffset", m_servo_motor.cfg.hModeCfg.homeOffset, 0);
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " homeOffset is " << m_servo_motor.cfg.hModeCfg.homeOffset);
    }
  
    return true;
}

/*Description: motor init(include motor enable,gohome and switch ctlmode)*/
int ServoNewstart::Motor_Init()
{
    uint8_t nodeId = m_servo_motor.cfg.nodeId;
    if((0 == nodeId) || (nodeId > 127))
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] motor nodeId is unvaild", joint_name.c_str());
        return -1;       
    }

    /* 2023.9.19 added: canopen refactor.   ---> */
    // set_control_word_pub  = nh_.advertise<std_msgs::msg::UInt16>("/device" + std::to_string(nodeId) + "/set_controlword", 10);
    set_vel_pub = node_->create_publisher<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_target_velocity", 10);
    set_pos_pub = node_->create_publisher<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_target_position", 10);
    set_control_word_sdo_pub = node_->create_publisher<std_msgs::msg::UInt16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_controlword_sdo", 10);
    set_di_sdo_pub = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_digital_inputs_sdo", 10);
    set_home_offset_mode_sdo_pub = node_->create_publisher<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_group_homing_speed/home_offset_mode_sdo", 10);
    set_homing_switch_speed_sdo_pub = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_group_homing_speed/homing_speeds_speed_during_search_for_switch_1_sdo", 10);
    set_homing_zero_speed_sdo_pub = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_group_homing_speed/homing_speeds_speed_during_search_for_zero_sdo", 10);
    set_home_offset_sdo_pub = node_->create_publisher<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_home_offset_sdo", 10);
    set_homing_acc_sdo_pub = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_homing_accelaration_sdo", 10);
    set_homing_method_sdo_pub = node_->create_publisher<std_msgs::msg::UInt16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_homing_method_sdo", 10);
    set_mode_sdo_pub = node_->create_publisher<std_msgs::msg::Int8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_modes_of_operation_sdo", 10);
    set_acc_sdo_pub = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_profile_acceleration_sdo", 10);
    set_dec_sdo_pub = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_profile_deceleration_sdo", 10);
    set_p_vel_sdo_pub = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_profile_velocity_sdo", 10);
    set_pos_sdo_pub = node_->create_publisher<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_target_position_sdo", 10);

    mode_sub = node_->create_subscription<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_operation_mode_buff",
        10,
        std::bind(&ServoNewstart::mode_sub_callback, this, std::placeholders::_1));

    if (servo_type == JACKING || servo_type == ROTARY) {
        errcode_sub = node_->create_subscription<std_msgs::msg::UInt16>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_error_code_for_ds301",
            10,
            std::bind(&ServoNewstart::errcode_sub_callback, this, std::placeholders::_1));
        torque_sub = node_->create_subscription<std_msgs::msg::Int16>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_torque_actual_value",
            10,
            std::bind(&ServoNewstart::torque_sub_callback, this, std::placeholders::_1));
    } else if (servo_type == NORMAL) {
        errcode_sub = node_->create_subscription<std_msgs::msg::UInt16>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_error_code",
            10,
            std::bind(&ServoNewstart::errcode_sub_callback, this, std::placeholders::_1));
    }

    current_pos_sub = node_->create_subscription<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_position_actual_value_",
        10,
        std::bind(&ServoNewstart::current_pos_sub_callback, this, std::placeholders::_1));
    current_vel_sub = node_->create_subscription<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_speed_real",
        10,
        std::bind(&ServoNewstart::current_vel_sub_callback, this, std::placeholders::_1));
    status_word_sub = node_->create_subscription<std_msgs::msg::UInt16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_statusword",
        10,
        std::bind(&ServoNewstart::status_word_sub_callback, this, std::placeholders::_1));
    p_vel_sub = node_->create_subscription<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_profile_velocity",
        10,
        std::bind(&ServoNewstart::p_vel_sub_callback, this, std::placeholders::_1));
    timestamp_sub = node_->create_subscription<std_msgs::msg::Float64>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_position_actual_value_/timestamp",
        10,
        std::bind(&ServoNewstart::timestamp_sub_callback, this, std::placeholders::_1));

    RCLCPP_INFO(node_->get_logger(), 
        "[SERVO_MOTOR] %s subscribe prefix: %s", 
        joint_name.c_str(), std::string("/" + busname + "/device" + std::to_string(nodeId)).c_str());

    if (servo_type == JACKING) {
        target_pos_sub = node_->create_subscription<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_target_position", 
            10, 
            std::bind(&ServoNewstart::target_pos_sub_callback, this, std::placeholders::_1));
    }

    // homemachine
    if (!Motor_HomeMachine()) {
        return -1;
    }

    RCLCPP_INFO(node_->get_logger(), "node[%d] enable.", nodeId);
    // motor enable
    bool enableFlage = Motor_Enable(true);
    if(!enableFlage)
    {
        return -1;
    }

    // ctlmode
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set position mode.", nodeId);
        if (!Motor_InitPmode())
            return -1;
    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set velocity mode.", nodeId);
        if(!Motor_InitVmode())
            return -1;
    }
    else
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] unknown drive mode", joint_name.c_str());
        return -1;
    }
    motorState = Ready;        
    return 0;
}

/*Description: motor enable or disable*/
bool ServoNewstart::Motor_Enable(bool onFlag)
{
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        Motor_SetPmodeVel(0);

        /* 2023.9.19 modify: cnaopen refactor.   --->
        pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x607A, 0x00, 0x00);
        */
        std_msgs::msg::Int32 msg;
        msg.data = (int32_t)0x00;
        set_pos_sdo_pub->publish(msg);
        /* <--- modify end */

    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
    {
        Motor_SetTargetVelEnc(0);
    }

    uint8_t nodeId = m_servo_motor.cfg.nodeId;

    /* 2023.9.19 modify: cnaopen refactor.   --->
    // uint16_t statusWord = pCanopen->getStatusWord(nodeId);
    */
    uint16_t statusWord = pc_.status_word;
    /* <--- modify end */


    RCLCPP_INFO(node_->get_logger(), "node %d statusWord is %d",nodeId,statusWord);
    if(!onFlag)
    {
        if((statusWord & 0x07) != 0x07)
        {
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] motor has been disabled", joint_name.c_str());
            return true;
        }

        /* 2023.9.19 modify: cnaopen refactor.   --->
        pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x06);servo_modeSet
        */

        int timeout = 0;
        while ((statusWord & 0x07) == 0x07 && timeout < 10) {
            std_msgs::msg::UInt16 msg;
            msg.data = (uint16_t)0x06;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(500));
            statusWord = pc_.status_word;
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] checkout status_word[0x%x] trys: %d", joint_name.c_str(), statusWord, timeout);
            timeout++;
        }

        /* <--- modify end */

        /* 2023.9.19 modify: cnaopen refactor.   --->
        statusWord = pCanopen->getStatusWord(nodeId);
        */
        /* <--- modify end */

        if((statusWord & 0x07) == 0x07)
        {
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] motor failed to be disable", joint_name.c_str());
        }
        else
        {
            RCLCPP_WARN(node_->get_logger(), "[SERVO_MOTOR] servo[%s] motor was disabled successfully", joint_name.c_str());               
            return false; 
        }
    }
    else
    {
        if((statusWord & 0x07) == 0x07)
        {
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] motor has been enabled", joint_name.c_str());
            return true;
        }

        /* 2023.9.19 modify: cnaopen refactor.   --->
        pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x86); //reset err
        rclcpp::Duration(1).sleep();
        pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x06);
        pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x07);
        pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x0F);
        rclcpp::Duration(1).sleep();
        statusWord = pCanopen->getStatusWord(nodeId);
        */
        

        int timeout = 0;
        while ((statusWord & 0x07) != 0x07 && timeout < 10) {
        
            std_msgs::msg::UInt16 msg;
            msg.data = (uint16_t)0x86;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(5));
            msg.data = (uint16_t)0x06;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(5));
            msg.data = (uint16_t)0x07;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(5));
            msg.data = (uint16_t)0x0F;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(500));
            statusWord = pc_.status_word;

            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] checkout status_word[0x%x] trys: %d", joint_name.c_str(), statusWord, timeout);
            timeout++;
        }
        /* <--- modify end */

        if((statusWord & 0x07) == 0x07)
        {
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] motor was enabled successfully", joint_name.c_str());
        }
        else
        {
            RCLCPP_WARN(node_->get_logger(), "[SERVO_MOTOR] servo[%s] motor failed to be enabled: 0x%x", joint_name.c_str(), statusWord);                
            return false;
        }
    }

    return true; 
}


/*Description: configure motor homing parameters and excute*/
bool ServoNewstart::Motor_HomeMachine()
{
    uint16_t statusWord = 0;
    if(!m_servo_motor.cfg.homeNeed) //todo get 609903 check whether the system automatically returns to zero
    {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR]motor does not need to go home");
        return true;
    }
    if(!m_servo_motor.cfg.hModeCfg.home_mode)
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]motro home mode is not define");
        return false;
    }
    servo_modeSet(Homing);
    servo_homeMethodSet(m_servo_motor.cfg.hModeCfg.home_mode);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    servo_homeMethodSet(m_servo_motor.cfg.hModeCfg.home_mode);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    servo_homeMethodSet(m_servo_motor.cfg.hModeCfg.home_mode);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    servo_homeOffsetModeSet(0); // todo mode add cfg
    servo_homeOffsetSet(m_servo_motor.cfg.hModeCfg.homeOffset);
    servo_homeSpeedHSet(m_servo_motor.cfg.hModeCfg.velH);
    servo_homeSpeedLSet(m_servo_motor.cfg.hModeCfg.velL);
    servo_homeAccSet(m_servo_motor.cfg.hModeCfg.acc);
    rclcpp::sleep_for(std::chrono::milliseconds(10));
    servo_homeStart();
    rclcpp::sleep_for(std::chrono::milliseconds(1000));    // wait homing start and status word updated
    rclcpp::Time homing_start_time = node_->now();
    do{
        /* 2023.9.19  modify: canopen refactor   --->
        statusWord = pCanopen->getStatusWord(m_servo_motor.cfg.nodeId);
        */
        statusWord = pc_.status_word;
        /* <--- modify end */
        rclcpp::sleep_for(std::chrono::milliseconds(10));

        if((node_->now() - homing_start_time).seconds() > 40) //todo timeout
        {
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] home time out", joint_name.c_str());
            break;
        }
    } while(!(statusWord & 0x8000));

    if(statusWord & 0x8000)
    {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] home finished", joint_name.c_str());

        if (servo_type == ROTARY) {
            rotary_pos = 0.0;
            rotary_old_pos = 0.0;
            rotary_delta_pos = 0.0;
            RCLCPP_INFO(node_->get_logger(), "[ServoNewstart] reset rotary_pos: %.4f", rotary_pos);
        }
    }
    else
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] home failed", joint_name.c_str());
        Motor_Enable(false); //disable motor
        return false;       
    }
    return true;
}

/*Description: configure the motor for Profiled Velocity mode*/
bool ServoNewstart::Motor_InitVmode()
{
    Motor_Enable(false);
    servo_modeSet(Profiled_Velocity);
    Motor_Enable(true);
    if(Profiled_Velocity != Motor_GetMode())
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] failed to set the velocity mode", joint_name.c_str());
        return false;
    }
    Motor_SetAcc(m_servo_motor.cfg.vModeCfg.acc);
    Motor_SetDec(m_servo_motor.cfg.vModeCfg.dec);
    Motor_SetTargetVelEnc(0);
    
    return true;
}

/*Description: configure the motor for Profiled Position mode*/
bool ServoNewstart::Motor_InitPmode()
{
    Motor_Enable(false);
    servo_modeSet(Profiled_Position);
    Motor_Enable(true);
    if(Profiled_Position != Motor_GetMode())
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] failed to set the position mode", joint_name.c_str());
        return false;
    }
    Motor_SetAcc(m_servo_motor.cfg.pModeCfg.acc);
    Motor_SetDec(m_servo_motor.cfg.pModeCfg.dec);
    Motor_SetPmodeVel(m_servo_motor.cfg.pModeCfg.vel);

    // std_msgs::msg::UInt16 ctrlwd_msg;
    // ctrlwd_msg.data = (uint16_t)0x103F;
    // set_control_word_sdo_pub->publish(ctrlwd_msg);
    // rclcpp::sleep_for(std::chrono::milliseconds(5));
    // set_control_word_sdo_pub->publish(ctrlwd_msg);
    // rclcpp::sleep_for(std::chrono::milliseconds(5));
    // set_control_word_sdo_pub->publish(ctrlwd_msg);
    
    return true;
}

/*Description: switch motor mode, currently only supported Profiled Position mode and Profiled Velocity mode*/
bool ServoNewstart::Motor_SwitchMode(int8_t mode)
{
    switch(mode)
    {
        case Profiled_Position:
            Motor_InitPmode();
            break;
        case Profiled_Velocity:
            Motor_InitVmode();
            break;
        case Homing:
            break;
        default:
            RCLCPP_WARN(node_->get_logger(), "servo[%s] undefined control mode", joint_name.c_str());
            break;
    }
    int8_t actualCtlMode = Motor_GetMode();
    if(actualCtlMode != mode)
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] failed to set the ctl mode", joint_name.c_str());
        return false;
    }
    return true;
}

/*Description: set motor acceleration*/
void ServoNewstart::Motor_SetAcc(uint32_t acc)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6083, 0x00, acc);   
    */
    std_msgs::msg::UInt32 msg;
    msg.data = acc;
    set_acc_sdo_pub->publish(msg);
    /* <--- modify end */
}

/*Description: set motor deceleration*/
void ServoNewstart::Motor_SetDec(uint32_t dec)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6084, 0x00, dec);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = dec;
    set_dec_sdo_pub->publish(msg);
    /* <--- modify end */
}

/*Description: set the speed for Profiled Position mode*/
void ServoNewstart::Motor_SetPmodeVel(uint32_t vel)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6081, 0x00, vel);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = vel;
    set_p_vel_sdo_pub->publish(msg);
    /* <--- modify end */
}

/*Description: set the speed for Profiled Velocity mode, vel is expressed in rad/s*/
void ServoNewstart::Motor_SetTargetVelRad(double vel)
{
    if(Ready != Motor_GetState())
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()),
            1000, "[ServoNewstart] motor[%s] state[%d] not Ready !!!", joint_name.c_str(), Motor_GetState());
        return;
    }
    /* <--- add end */

    if(Motor_GetMode() != Profiled_Velocity)
    {
        return;
    } 
    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()),
            1000, "[ServoNewstart] motor[%s] reduction_ratio||encoder_resolution is 0.", joint_name.c_str());
        return;
    }
    //RCLCPP_INFO(node_->get_logger(), "vel is %lf",vel);
    
    if (std::isnan(vel) || abs(vel) > 1e9) {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()),
            1000, "[ServoNewstart] bad vel value: %.4f", vel);
        Motor_SetTargetVelEnc(0);
        return;
    }


    const double newstart_ratio  = 30 / M_PI * m_servo_motor.cfg.reduction_ratio;
    const double mairui_ratio = 10 * m_servo_motor.cfg.reduction_ratio * m_servo_motor.cfg.encoder_resolution / 2 / M_PI;

    int targetVel = 0;
    if (servo_type == JACKING) {
        vel /= rad_to_meter;
        targetVel = vel * newstart_ratio;
    } 
    else if (servo_type == ROTARY) {
        targetVel = vel * newstart_ratio;
    }
    else if (servo_type == NORMAL) {
        targetVel = vel * mairui_ratio; 
    }
    else {
        targetVel = 0;
    }

    Motor_SetTargetVelEnc(targetVel);
}

/*Description: set the speed for Profiled Velocity mode, vel is expressed in encoder*/
void ServoNewstart::Motor_SetTargetVelEnc(int vel)
{
    std_msgs::msg::Int32 msg;
    msg.data = vel;
    set_vel_pub->publish(msg);
}

/*Description: set the target pos for Absolute position control mode, pos is expressed in encoder*/
void ServoNewstart::Motor_SetTargetPosEnc(int pos)
{
    if(Ready != Motor_GetState())
    {
        return;
    }
    int encoder = servo_encoderGet();
    // RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR]get encoder %d",encoder);
    if(abs(pos - encoder) < 100)
    {
        return;
    }

    if(pos == preTargetEnc)
    {
        return;
    }


    if ((pc_.status_word >> 10) & 0x1) {
        if ((pos_send_scale % 10) == 0) {
            std_msgs::msg::Int32 pos_msg;
            pos_msg.data = pos;
            set_pos_sdo_pub->publish(pos_msg);

            rclcpp::sleep_for(std::chrono::milliseconds(2));
            std_msgs::msg::UInt16 ctrlwd_msg;
            ctrlwd_msg.data = (uint16_t)0x0F;
            set_control_word_sdo_pub->publish(ctrlwd_msg);
            
            rclcpp::sleep_for(std::chrono::milliseconds(2));
            ctrlwd_msg.data = (uint16_t)0x5F;
            set_control_word_sdo_pub->publish(ctrlwd_msg);

            RCLCPP_INFO_STREAM(node_->get_logger(), 
                "[ServoNewstart] set target pos enc: " << pos << "  statusWord: " << pc_.status_word);    
        }

        pos_send_scale++;
        if (pos_send_scale > 9)
            pos_send_scale = 0;
    }
    else {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            500, "[ServoNewstart] %s busy.", joint_name.c_str());
        pos_send_scale = 0;
        preTargetEnc = pos;
    }


   
}

/*Description: set the target pos for Absolute position control mode, pos is expressed in rad*/
void ServoNewstart::Motor_SetTargetPosRad(double pos)
{
    // ROS_INFO_STREAM("target pos rad is "<<pos);
    if(Ready != Motor_GetState())
    {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] motor [%s] state[%d] not Ready !!!", joint_name.c_str(), Motor_GetState());
        return;
    }
    if(Motor_GetMode() != Profiled_Position)
    {
        // RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] motor mode[%d] not Profiled_Position !!!", Motor_GetMode());
        return;
    }
    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] reduction_ratio = 0 || encoder_resolution = 0");
        return;
    }

     /* 2023.8.17 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        pos /= rad_to_meter;
    } else {
        // continuous: do nothing
    }

    /* <--- and end */


    //int targetEnc = pos / 2 / M_PI * m_servo_motor.cfg.encoder_resolution * m_servo_motor.cfg.reduction_ratio;
    double radToEnc = 2.0 * M_PI / m_servo_motor.cfg.encoder_resolution / m_servo_motor.cfg.reduction_ratio;
    int targetEnc = 0;
    if (abs(radToEnc) > 1e-10)
        targetEnc = pos / radToEnc;
    else 
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] radToEnc zero: %f", joint_name.c_str(), radToEnc);

    int encoder = servo_encoderGet();
    // RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()),0.5, "[SERVO_MOTOR] get encoder: %d  targetEnc: %d",encoder, targetEnc);
    if(abs(targetEnc - encoder) < 100)
    {
        return;
    }
    Motor_SetTargetPosEnc(targetEnc);
}

/*Description: set the target pos for relative position control mode, pos is expressed in encoder*/
void ServoNewstart::servo_PRelMove(int data)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_607A00, data);
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_604000, 0x4F);
    rclcpp::sleep_for(std::chrono::milliseconds(30));
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_604000, 0x5F);
    */
    std_msgs::msg::Int32 pos_msg;
    pos_msg.data = data;
    set_pos_pub->publish(pos_msg);

    std_msgs::msg::UInt16 ctrlwd_msg;
    ctrlwd_msg.data = (uint16_t)0x4F;
    set_control_word_pub->publish(ctrlwd_msg);

    rclcpp::sleep_for(std::chrono::milliseconds(30));
    ctrlwd_msg.data = (uint16_t)0x5F;
    set_control_word_pub->publish(ctrlwd_msg);
    /* <---  modify end */
}


/*Description: get the motor speed ignore control mode, pos is expressed in rad per second*/
double ServoNewstart::Motor_GetCurrentVel()
{
    int vel;
    double retVel;
    const double newstart_ratio  = 30 / M_PI * m_servo_motor.cfg.reduction_ratio;
    const double mairui_ratio = 10 * m_servo_motor.cfg.reduction_ratio * m_servo_motor.cfg.encoder_resolution / 2 / M_PI;
    
    vel = servo_VvelGet();
    
    /* 2023.8.18 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        retVel = vel / newstart_ratio;
        // retVel *= rad_to_meter;
    }
    else if (servo_type == ROTARY) {
        // continuous: do nothing
        retVel = vel /newstart_ratio;
    }
    else if (servo_type == NORMAL) {
        retVel = vel / mairui_ratio;
    }
    else {
        retVel = 0.0;
    }
    
    return retVel;
}

/*Description: get the motor speed ignore control mode*/
int ServoNewstart::servo_velGet()
{
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        return servo_PvelGet();
    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
    {
        return servo_VvelGet();
    }
    else
        return 0;
}

/*Description: get the motor speed in position control mode*/
uint32_t ServoNewstart::servo_PvelGet()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getPvel(m_servo_motor.cfg.nodeId);
    */
    return pc_.profile_vel;
    /* <--- modify end */
}

/*Description: get the motor speed in velocity control mode*/
int ServoNewstart::servo_VvelGet()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getVvel(m_servo_motor.cfg.nodeId, servo_type);
    */
    return pc_.current_vel;
    /* <--- modify end */
}

/*Description: get the motor position,unit is rad*/
double ServoNewstart::Motor_GetCurrentPos()
{
    int encoder = servo_encoderGet();
    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        return 0;
    }
    const double encTorad = m_servo_motor.cfg.encoder_resolution * m_servo_motor.cfg.reduction_ratio / 2 / M_PI;
    double pos = encoder / encTorad;

    /* 2023.8.18 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        pos *= rad_to_meter;
    } 
    else if (servo_type == ROTARY) {
        rotary_delta_pos = pos - rotary_old_pos;
        if (abs(rotary_delta_pos) > 1.0) {
            rotary_pos = rotary_old_pos;
        }
        else {
            rotary_pos += rotary_delta_pos;
        }

        rotary_old_pos = rotary_pos;
        return rotary_pos;
    }
    else {
        // continuous: do nothing
    }

    return pos;
}

/*Description: get the motor encoder*/
int ServoNewstart::servo_encoderGet()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getEnc(m_servo_motor.cfg.nodeId);
    */
    return pc_.current_pos;
    /* <--- modify end */
}

/*Description: get the motor status word*/
uint16_t ServoNewstart::Motor_GetStatusWord()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getStatusWord(m_servo_motor.cfg.nodeId);
    */
    return pc_.status_word;
    /* <--- modify end */
}

std::string ServoNewstart::Motor_Err2userErr(const uint16_t& motorErr)
{
    std::string errInfo;
    switch(motorErr)
    {
        case 0x0000: errInfo = "no err"; break;
        case 0x0001: errInfo = "Current too high"; break;
        case 0x0002: errInfo = "Voltage too high"; break;
        case 0x0003: errInfo = "Voltage too low"; break;
        case 0x0004: errInfo = "Temperatur too high"; break;
        case 0x0005: errInfo = "Motor over load"; break;
        case 0x0006: errInfo = "Velocity too high"; break;
        case 0x0007: errInfo = "Bad parameter "; break;
        case 0x0008: errInfo = "Uncontrolled error"; break;
        case 0x0009: errInfo = "Position following error"; break;
        case 0x000a: errInfo = "Position overflow error"; break;
        case 0x000b: errInfo = "Hardware current overload"; break;
        case 0x000c: errInfo = "Driver forbit error"; break;
        case 0x000d: errInfo = "Motor stalling"; break;
        case 0x000e: errInfo = "Power supply error"; break;
        case 0x000f: errInfo = "STO error"; break;
        case 0x0010: errInfo = "Phase current zero adjust error"; break;
        case 0x0011: errInfo = "EEPROM error"; break;
        case 0x0012: errInfo = "Hall sensor error"; break;
        case 0x0013: errInfo = "Encoder error"; break;
        case 0x0014: errInfo = "Encoder calib zero error"; break;
        case 0x0015: errInfo = "Encoder signal Z lost"; break;
        case 0x0016: errInfo = "Encoder calculate error"; break;
        case 0x0017: errInfo = "Encoder cycles overflow"; break;
        case 0x0018: errInfo = "External clock error"; break;
        case 0x0019: errInfo = "UVW order error"; break;
        case 0x001a: errInfo = "FPGA error"; break;
        case 0x001b: errInfo = "Return zero error"; break;
        case 0x001c: errInfo = "Encoder component error"; break;
        case 0x001d: errInfo = "Servo power lost error"; break;
        case 0x001e: errInfo = "Ethernet error"; break;
        case 0x001f: errInfo = "Ethernet SM DOG error"; break;
        case 0x0020: errInfo = "Ethernet FATAL SYNC error"; break;
        case 0x0021: errInfo = "Ethernet SYNC error"; break;
        case 0x0022: errInfo = "Ethernet RFT error"; break;
        case 0x0023: errInfo = "Driver axle address error"; break;
        case 0x0024: errInfo = "Motor homing error"; break;
        case 0x0025: errInfo = "Encoder communication error"; break;
        case 0x0026: errInfo = "Encoder component error"; break;
        case 0x0027: errInfo = "Encoder component error"; break;
        case 0x0028: errInfo = "Encoder component error"; break;
        case 0x0029: errInfo = "Encoder component error"; break;
        case 0x002a: errInfo = "Encoder component error"; break;
        case 0x002b: errInfo = "Encoder component error"; break;
        case 0x002c: errInfo = "Driver type error"; break;
        case 0x002d: errInfo = "Encoder collision detection error"; break;
        case 0x002e: errInfo = "Driver axle number error"; break;
        case 0x002f: errInfo = "Collision warning"; break;
        default:     errInfo = "unknown warning"; break;
    }
    return errInfo;
}

/*Description: get the motor error code from object 603f*/
uint16_t ServoNewstart::Motor_GetErrCode()
{ 
    /* 2023.9.19  modify: canopen refactor.   --->
    uint16_t errCode = errCode = pCanopen->getErrCode(m_servo_motor.cfg.nodeId);
    */
    uint16_t errCode = pc_.error_code;
    /* <--- modify end */
    if(Ready == Motor_GetState())
    {
        if(errCode)
        {
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()),
                1000, "[ServoKinco] errcode: %d", errCode);
            motorState = Error;
        }
    }
    else {
        if (Error == Motor_GetState() && errCode == 0) {
            motorState = Ready;
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()),
                1000, "[ServoKinco] errcode clear");   
        }
    }
    return errCode;
}

/*Description: get the motor error code from object 603f*/
uint32_t ServoNewstart::servo_DiStatusGet()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getDiStatus(m_servo_motor.cfg.nodeId, servo_type);
    */
    return pc_.digital_input;
    /* <--- modify end */
}

/*Description: get the status of the drive digital input signal*/
void ServoNewstart::servo_ErrClear()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x86);
    */
    std_msgs::msg::UInt16 msg;
    msg.data = (uint16_t)0x86;
    set_control_word_sdo_pub->publish(msg);
    /* <--- modify end */
}


/*Description: motor electric brake control,only zapi motor are supported*/
void ServoNewstart::Motor_Brake(bool on)
{

}

/*Description: shutdown servo*/
void ServoNewstart::Motor_Shutdown()
{
    return;
    /* 2023.9.19  delete: canopen refactor.   --->
    pCanopen->nmtSend(m_servo_motor.cfg.nodeId, Canopen::State::Stopped);
    */
}

bool ServoNewstart::Motor_Halt()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x0B);   
    */
    motorState = Halt;
    std_msgs::msg::UInt16 msg;
    msg.data = 0x06;
    set_control_word_sdo_pub->publish(msg);
    return true;
    /* <--- modify end */
}

/*Description: reset servo error and enable servo*/
bool ServoNewstart::Motor_Recover()
{
    uint8_t nodeId = m_servo_motor.cfg.nodeId;
    if(motorState != Recover)
    {
        motorState = Recover;

        /* 2023.9.19  modify: canopen refactor.   --->
        pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x86); //reset err
        rclcpp::Duration(1).sleep();
        uint16_t errcode = pCanopen->getErrCode(m_servo_motor.cfg.nodeId);
        */
        uint16_t errcode = pc_.error_code;

        std_msgs::msg::UInt16 msg;
        msg.data = 0x86;
        set_control_word_sdo_pub->publish(msg);
        rclcpp::sleep_for(std::chrono::milliseconds(200));
        
        int timeout = 0;
        while (errcode && timeout < 10) {
            msg.data = 0x86;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(200));
            errcode = pc_.error_code;
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] checkout error_code[0x%x] trys: %d", joint_name.c_str(), errcode, timeout);
            timeout++;
        }
        /* <--- modify end */

        if(!errcode)
        {
            /* 2023.9.19  modify: canopen refactor.   --->
            pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x06);
            pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x07);
            pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x0F);
            rclcpp::Duration(1).sleep();
            uint16_t statusWord = pCanopen->getStatusWord(nodeId);
            */

            uint16_t statusWord = pc_.status_word;
                
            timeout = 0;
            while ((statusWord & 0x07) != 0x07 && timeout < 20) {
                msg.data = 0x06;
                set_control_word_sdo_pub->publish(msg);

                rclcpp::sleep_for(std::chrono::milliseconds(5));
                msg.data = 0x07;
                set_control_word_sdo_pub->publish(msg);
                
                rclcpp::sleep_for(std::chrono::milliseconds(5));
                msg.data = 0x0F;
                set_control_word_sdo_pub->publish(msg);
                rclcpp::sleep_for(std::chrono::milliseconds(500));;

                statusWord = pc_.status_word;
                RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] checkout status_word[0x%x] trys: %d", joint_name.c_str(), statusWord, timeout);
                timeout++;
            }

            /* <--- modify end */

            if((statusWord & 0x07) == 0x07)
            {
                // if (servo_type == JACKING) {
                //     std_msgs::msg::UInt16 ctrlwd_msg;
                //     ctrlwd_msg.data = (uint16_t)0x103F;
                //     set_control_word_sdo_pub->publish(ctrlwd_msg);
                //     rclcpp::sleep_for(std::chrono::milliseconds(5));
                //     set_control_word_sdo_pub->publish(ctrlwd_msg);
                //     rclcpp::sleep_for(std::chrono::milliseconds(5));
                //     set_control_word_sdo_pub->publish(ctrlwd_msg);
                //     RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] set control word 0x103f for jacking motor");
                // }
                if (servo_type == JACKING) {
                    rclcpp::sleep_for(std::chrono::milliseconds(2));
                    std_msgs::msg::UInt16 ctrlwd_msg;
                    ctrlwd_msg.data = (uint16_t)0x0F;
                    set_control_word_sdo_pub->publish(ctrlwd_msg);
                    
                    rclcpp::sleep_for(std::chrono::milliseconds(2));
                    ctrlwd_msg.data = (uint16_t)0x5F;
                    set_control_word_sdo_pub->publish(ctrlwd_msg);
                }
                
                motorState = Ready;
                RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] motor was enabled successfully", joint_name.c_str());
            }                    
        }
        else
        {
            motorState = Error;
            RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] error is sitll obtained after clear error", joint_name.c_str());
            return false;
        }
    }
    return true;
}


void ServoNewstart::servo_homeOffsetModeSet(uint8_t mode)
{
    if(mode > 1)
    {
        return;
    }

    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 1, 0x6099, 0x05, mode);    
    */
    std_msgs::msg::UInt8 msg;
    msg.data = mode;
    set_home_offset_mode_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoNewstart::servo_homeOffsetSet(int homeOffset)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x607c, 0x00, homeOffset);
    */
    std_msgs::msg::Int32 msg;
    msg.data = homeOffset;
    set_home_offset_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoNewstart::servo_homeMethodSet(int homeMethod)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 1, 0x6098, 0x00, homeMethod);
    */
    std_msgs::msg::UInt16 msg;
    msg.data = homeMethod;
    set_homing_method_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoNewstart::servo_homeSpeedHSet(uint32_t homeSpeedH)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6099, 0x01, homeSpeedH);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = homeSpeedH;
    set_homing_switch_speed_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoNewstart::servo_homeSpeedLSet(uint32_t homeSpeedL)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6099, 0x02, homeSpeedL);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = homeSpeedL;
    set_homing_zero_speed_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoNewstart::servo_homeAccSet(uint32_t homeAcc)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x609A, 0x00, homeAcc);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = homeAcc;
    set_homing_acc_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoNewstart::servo_homeStart()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x0F);
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x1F); 
    */
    std_msgs::msg::UInt16 msg;
    msg.data = 0x06;
    set_control_word_sdo_pub->publish(msg);
    msg.data = 0x07;
    set_control_word_sdo_pub->publish(msg);
    msg.data = 0x0F;
    set_control_word_sdo_pub->publish(msg);
    msg.data = 0x1F;
    set_control_word_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoNewstart::servo_readDiStatus()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x60FD, 0x00, 0x00);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = 0x0;
    set_di_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoNewstart::servo_modeSet(uint8_t mode)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 1, 0x6060, 0x00, mode);
    */

    int timeout = 0;
    while (Motor_GetMode() != mode && timeout < 20) {
        std_msgs::msg::Int8 msg;
        msg.data = mode;
        set_mode_sdo_pub->publish(msg);
        rclcpp::sleep_for(std::chrono::milliseconds(500));
        
        RCLCPP_INFO(node_->get_logger(), "set mode: %d  current: % d  trys: %d", mode, Motor_GetMode(), timeout);
        timeout++;
    }
    /* <--- modify end */
}

int8_t ServoNewstart::Motor_GetMode()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getActualCtlMode(m_servo_motor.cfg.nodeId);
    */
    return pc_.mode;
}


MotorState ServoNewstart::Motor_GetState() 
{
    return motorState;
}

double ServoNewstart::Motor_GetCurrent()
{
    if (servo_type == JACKING || servo_type == ROTARY) {
        double result = 0.0;
        result = ((double)pc_.torque / 1000.0) * ipeak;
        return result;
    }
    else {
        return 0.0;
    }
}


void ServoNewstart::mode_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg)
{
    pc_.mode = msg->data;
}

void ServoNewstart::errcode_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg)
{
    pc_.error_code = msg->data;
}

void ServoNewstart::current_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg)
{
    pc_.current_pos = msg->data;
}

void ServoNewstart::current_vel_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg)
{
    pc_.current_vel = msg->data;
}

void ServoNewstart::status_word_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg)
{
    pc_.status_word = msg->data;
}

void ServoNewstart::di_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg)
{
    pc_.digital_input = msg->data;
}

void ServoNewstart::p_vel_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg)
{
    pc_.profile_vel = msg->data;
}

void ServoNewstart::target_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg)
{
    pc_.target_pos = msg->data;
}

void ServoNewstart::torque_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg)
{
    pc_.torque = msg->data;
}

void ServoNewstart::timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr& msg)
{
    pc_.timestamp = msg->data;
}

}